import cv2
import numpy as np
import matplotlib.pyplot as plt

# 相机参数 (需替换为实际标定值)
fx = 1000.0   # 焦距x
fy = 1000.0   # 焦距y
cx = 190    # 主点x
cy = 105    # 主点y
baseline = 0.12  # 双目基线（米）

# 加载左右视图
img_left = cv2.imread('./left.jpg', 0)
img_right = cv2.imread('./right.jpg', 0)

# 特征提取与匹配
detector = cv2.ORB_create()
matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)

kp_left, des_left = detector.detectAndCompute(img_left, None)
kp_right, des_right = detector.detectAndCompute(img_right, None)
# plt.figure(),plt.imshow(kp_left)
matches = matcher.match(des_left, des_right)
matches = sorted(matches, key=lambda x: x.distance)[:100]  # 取前100个最佳匹配

# 提取匹配点坐标
pts_left = np.float32([kp_left[m.queryIdx].pt for m in matches]).reshape(-1, 2)
pts_right = np.float32([kp_right[m.trainIdx].pt for m in matches]).reshape(-1, 2)

# 视差计算与三角化
disparities = pts_left[:, 0] - pts_right[:, 0]  # 计算水平视差
Z = (fx * baseline) / (disparities + 1e-5)  # 深度计算

# 转换为三维坐标 (左相机坐标系)
X = (pts_left[:, 0] - cx) * Z / fx
Y = (pts_left[:, 1] - cy) * Z / fy
points_3d = np.vstack((X, Y, Z)).T

# 使用plt展示
fig = plt.figure()
ax = fig.add_subplot(111,projection='3d')
ax.scatter(points_3d[:, 0], points_3d[:, 1], points_3d[:, 2])
plt.show()


# 保存为PLY点云
with open('output.ply', 'w') as f:
    f.write("ply\n")
    f.write("format ascii 1.0\n")
    f.write(f"element vertex {len(points_3d)}\n")
    f.write("property float x\n")
    f.write("property float y\n")
    f.write("property float z\n")
    f.write("end_header\n")
    for p in points_3d:
        f.write(f"{p[0]} {p[1]} {p[2]}\n")
